#
# Class for sending to flightgear
#
import struct
import socket
import math

import pat.vehicles.fixed_wing.dynamic_model_3d as dm


class FlightGear:
    def __init__(self, addr, port, _sim):
        self.s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.addr = addr
        self.port = port
        _sim.connect("updated", self.update)

    def update(self, _sim):
        X = _sim._dm.X
        self.send(X)

    def send(self, X):
        earth_radius = 6372795.;

        lat =  0.656480 + math.asin((X[dm.s_x] - 90)/earth_radius);
        lon = -2.135537 + math.asin((X[dm.s_y] - 45)/earth_radius);

        fmt = "LL ddfffff ff Lffff LLf ffLfff"
        version = 8
        padding = 0
        latitude = lat
        longitude = lon
        altitude = -X[dm.s_z] + 3
        agl = 1.111652
        phi = X[dm.s_phi]
        theta = X[dm.s_theta]
        psi = X[dm.s_psi]
        vcas = 0.
        climb_rate = 0.
        num_tanks = 1
        fuel_quant1 = 0.
        fuel_quant2 = 0.
        fuel_quant3 = 0.
        fuel_quant4 = 0.
        cur_time = 3198060679
        warp = 1122474394
        ground_elev = 0.
        tuned_freq = 125.65
        nav_radial = 90.
        in_range = 1
        dist_nm = 10.
        course_deviation_deg = 0.
        gs_deviation_deg = 0.

        buf = struct.pack(fmt, version, padding,
                          longitude, latitude, altitude, agl, phi, theta, psi,
                          vcas, climb_rate,
                          num_tanks, fuel_quant1, fuel_quant2, fuel_quant3, fuel_quant4,
                          cur_time, warp, ground_elev, tuned_freq, nav_radial, in_range,
                          dist_nm, course_deviation_deg, gs_deviation_deg)
        self.s.sendto(buf, (self.addr, self.port))
